#include <descriptor-projection/build-projection-matrix.h>
#include <descriptor-projection/flags.h>
#include <maplab-common/test/testing-entrypoint.h>

TEST(PlacelessLoopClosure, ComputeCovariance) {
  Eigen::MatrixXf A;
  A.resize(13, 10);
  A << 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0,
      0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
      0, 0, 1, 0, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 1, 1,
      1, 0, 1, 1, 0, 0, 0, 1, 1, 1, 1, 0, 1, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1,
      0, 0, 1, 1, 1, 1;

  A.transposeInPlace();

  Eigen::MatrixXf cov;
  descriptor_projection::ComputeCovariance(A, &cov);

  Eigen::MatrixXf cov_gt;
  cov_gt.resize(10, 10);
  cov_gt << 0.230769230769231, -0.044871794871795, 0.070512820512821,
      -0.012820512820513, 0.038461538461538, 0.012820512820513,
      -0.096153846153846, -0.096153846153846, -0.096153846153846,
      -0.012820512820513,  // NOLINT
      -0.044871794871795, 0.256410256410256, 0.108974358974359,
      0.108974358974359, 0.089743589743590, 0.057692307692308,
      0.025641025641026, 0.025641025641026, 0.025641025641026,
      -0.141025641025641,  // NOLINT
      0.070512820512821, 0.108974358974359, 0.269230769230769,
      0.102564102564103, 0.025641025641026, 0.147435897435897,
      -0.064102564102564, -0.064102564102564, -0.064102564102564,
      -0.064102564102564,  // NOLINT
      -0.012820512820513, 0.108974358974359, 0.102564102564103,
      0.269230769230769, 0.108974358974359, 0.147435897435897,
      0.019230769230769, 0.019230769230769, 0.019230769230769,
      -0.147435897435897,  // NOLINT
      0.038461538461538, 0.089743589743590, 0.025641025641026,
      0.108974358974359, 0.256410256410256, 0.141025641025641,
      0.025641025641026, 0.025641025641026, 0.025641025641026,
      -0.141025641025641,  // NOLINT
      0.012820512820513, 0.057692307692308, 0.147435897435897,
      0.147435897435897, 0.141025641025641, 0.269230769230769,
      -0.019230769230769, -0.019230769230769, -0.019230769230769,
      -0.102564102564103,  // NOLINT
      -0.096153846153846, 0.025641025641026, -0.064102564102564,
      0.019230769230769, 0.025641025641026, -0.019230769230769,
      0.269230769230769, 0.185897435897436, 0.185897435897436,
      0.102564102564103,  // NOLINT
      -0.096153846153846, 0.025641025641026, -0.064102564102564,
      0.019230769230769, 0.025641025641026, -0.019230769230769,
      0.185897435897436, 0.269230769230769, 0.185897435897436,
      0.019230769230769,  // NOLINT
      -0.096153846153846, 0.025641025641026, -0.064102564102564,
      0.019230769230769, 0.025641025641026, -0.019230769230769,
      0.185897435897436, 0.185897435897436, 0.269230769230769,
      0.102564102564103,  // NOLINT
      -0.012820512820513, -0.141025641025641, -0.064102564102564,
      -0.147435897435897, -0.141025641025641, -0.102564102564103,
      0.102564102564103, 0.019230769230769, 0.102564102564103,
      0.269230769230769;  // NOLINT

  EXPECT_TRUE(cov.isApprox(cov_gt, 1e-3)) << "Ours: " << std::endl
                                          << cov << std::endl
                                          << " theirs: " << std::endl
                                          << cov_gt << std::endl;
}

TEST(PlacelessLoopClosure, ComputeProjectionMatrix) {
  Eigen::MatrixXf cM;
  cM.resize(10, 10);
  cM << 0.080480255686673, -0.000441449701280, -0.009146116233744,
      0.014215212362479, -0.012082301023504, 0.012614859877124,
      0.002064404840258, 0.008926947865386, 0.025895370233265,
      0.013561909921406,  // NOLINT
      -0.000441449701280, 0.086601899574966, -0.006880603349540,
      0.010636733766265, -0.014685987378964, 0.023230820055136,
      -0.009018726040167, 0.012690236201644, -0.011738699625274,
      0.001424174172984,  // NOLINT
      -0.009146116233744, -0.006880603349540, 0.082993331186798,
      0.006681152164491, 0.010362361703414, 0.011907651415983,
      -0.028835865829275, -0.026761612148162, -0.003470894301210,
      0.024394888323906,  // NOLINT
      0.014215212362479, 0.010636733766265, 0.006681152164491,
      0.080301835119374, -0.014900077112739, 0.009649537215215,
      -0.015204560085858, -0.023205247201078, 0.008007811473832,
      0.001065044272681,  // NOLINT
      -0.012082301023504, -0.014685987378964, 0.010362361703414,
      -0.014900077112739, 0.090742739005330, -0.014946586658111,
      0.004551011319820, -0.007864712090169, -0.027109222017290,
      0.013355205594321,  // NOLINT
      0.012614859877124, 0.023230820055136, 0.011907651415983,
      0.009649537215215, -0.014946586658111, 0.078442675218087,
      -0.013923130742838, -0.009948156253438, 0.010679225060752,
      0.000343952028387,  // NOLINT
      0.002064404840258, -0.009018726040167, -0.028835865829275,
      -0.015204560085858, 0.004551011319820, -0.013923130742838,
      0.087557465989724, 0.002835703295126, 0.003137663327051,
      -0.013179865559665,  // NOLINT
      0.008926947865386, 0.012690236201644, -0.026761612148162,
      -0.023205247201078, -0.007864712090169, -0.009948156253438,
      0.002835703295126, 0.079433208453810, 0.000585133811126,
      -0.010305807905903,  // NOLINT
      0.025895370233265, -0.011738699625274, -0.003470894301210,
      0.008007811473832, -0.027109222017290, 0.010679225060752,
      0.003137663327051, 0.000585133811126, 0.100335734049084,
      -0.003479493472938,  // NOLINT
      0.013561909921406, 0.001424174172984, 0.024394888323906,
      0.001065044272681, 0.013355205594321, 0.000343952028387,
      -0.013179865559665, -0.010305807905903, -0.003479493472938,
      0.084111559632430;  // NOLINT

  Eigen::MatrixXf cN;
  cN.resize(10, 10);
  cN << 0.081380937053169, 0.017941969505700, 0.012939949281583,
      0.011560657453582, 0.011348449244823, -0.007984497726697,
      -0.030325854907503, 0.001107159363540, -0.001137965914218,
      -0.026561981677408,  // NOLINT
      0.017941969505700, 0.069958970205704, -0.007147749103801,
      0.025033449278821, -0.004890544098532, -0.015657559978328,
      -0.004722766331671, -0.003842081890013, -0.001966881246292,
      0.009385653245263,  // NOLINT
      0.012939949281583, -0.007147749103801, 0.070994686862543,
      -0.003044112019506, -0.002333482195551, 0.015641043871002,
      -0.021719510862199, 0.021284579578366, -0.000457355436067,
      -0.020848946877098,  // NOLINT
      0.011560657453582, 0.025033449278821, -0.003044112019506,
      0.060719012236016, -0.005089706399327, -0.014463330515456,
      -0.004863817565195, -0.006020803669654, -0.011672429637451,
      -0.002884048145937,  // NOLINT
      0.011348449244823, -0.004890544098532, -0.002333482195551,
      -0.005089706399327, 0.088925109563472, -0.006784233120387,
      -0.006299388006171, 0.007466537472908, -0.006222649449800,
      0.008830900939307,  // NOLINT
      -0.007984497726697, -0.015657559978328, 0.015641043871002,
      -0.014463330515456, -0.006784233120387, 0.089738863120527,
      -0.009582567772864, 0.018388816189097, -0.004371980377919,
      0.013600856533082,  // NOLINT
      -0.030325854907503, -0.004722766331671, -0.021719510862199,
      -0.004863817565195, -0.006299388006171, -0.009582567772864,
      0.070354908417719, -0.010358653012276, -0.011480328946501,
      -0.007456975487622,  // NOLINT
      0.001107159363540, -0.003842081890013, 0.021284579578366,
      -0.006020803669654, 0.007466537472908, 0.018388816189097,
      -0.010358653012276, 0.083152361055459, -0.011436504521476,
      -0.000279449233913,  // NOLINT
      -0.001137965914218, -0.001966881246292, -0.000457355436067,
      -0.011672429637451, -0.006222649449800, -0.004371980377919,
      -0.011480328946501, -0.011436504521476, 0.079184032910998,
      0.026108731141316,  // NOLINT
      -0.026561981677408, 0.009385653245263, -0.020848946877098,
      -0.002884048145937, 0.008830900939307, 0.013600856533082,
      -0.007456975487622, -0.000279449233913, 0.026108731141316,
      0.085108973207343;  // NOLINT

  Eigen::MatrixXf A_gt;
  A_gt.resize(10, 10);
  A_gt << 6.294536081190175, 3.786900021705071, 3.038604179710021,
      -2.186961196040787, 0.589186929801228, -5.292534905420590,
      -1.415163093453460, -3.286711582047464, -0.691380298985069,
      -5.221727371465130,  // NOLINT
      0.640402228760257, -2.710597642121004, 4.306353628450472,
      1.772007484779011, -0.026849873052821, 2.830902745656463,
      1.617846417420010, 5.085978549219176, -2.011529866856202,
      -2.008635848946801,  // NOLINT
      0.007552716587578, 0.397873947009082, -0.327975432823965,
      0.541165922887131, 1.961098913720631, 0.142165498681882,
      -0.803249569395658, 1.047651154578566, 1.089498680061369,
      0.338570448206727,  // NOLINT
      0.779179055969222, -0.608055243645042, -0.908123736258334,
      -1.549818089006077, 0.064012494412123, 0.819454221489127,
      -1.051772728880867, -1.261367283510872, -0.164628849401058,
      -0.359374168128194,  // NOLINT
      0.048489597568037, -0.078138516885104, -0.193823710782691,
      0.147023285148408, 0.087109019794701, 0.120126629724251,
      0.009911687836391, -0.023154996941816, -0.172319662418592,
      0.009303590569603,  // NOLINT
      -0.331594552278296, 0.160981636707229, 0.570814334810170,
      -0.076000274383368, 1.028821584900522, 0.046118292673590,
      1.027521424449082, -0.171741184764752, 0.615163006864972,
      -0.650124257253199,  // NOLINT
      -0.875872877027125, -2.451209226273373, -1.112051089077660,
      1.218645655726876, -0.188518437497721, -1.200391698085745,
      -1.673710136390614, 0.719271309989150, -0.003650475575855,
      -0.236699925633316,  // NOLINT
      1.540367511018486, 0.761348121661040, -0.605271197361631,
      -3.880861186986408, -1.145305646955921, -2.000824737135372,
      0.055783001530438, 0.841207615535051, -1.902785963525169,
      1.260006773234884,  // NOLINT
      3.391652612969560, -2.312884557297318, -1.522290999682371,
      2.050812113744473, -0.648097244184962, 1.210294001490178,
      3.475964533824333, 2.570291788931910, 1.893411878904660,
      0.482848736591357,  // NOLINT
      4.231754860047078, -3.351594162183128, 4.477382121084116,
      0.797518176038082, -1.834162603460863, -1.703417155874213,
      3.879232918049624, -0.918746575144677, -3.020717068557894,
      5.473305932039536;  // NOLINT

  Eigen::MatrixXf A;
  descriptor_projection::ComputeProjectionMatrix(cM, cN, &A);

  EXPECT_TRUE(A.cwiseAbs().isApprox(A_gt.cwiseAbs(), 1e-3))
      << "Ours: " << std::endl
      << A << std::endl
      << "Theirs: " << std::endl
      << A_gt << std::endl;
}

MAPLAB_UNITTEST_ENTRYPOINT
